%% PSO Testing


%% Setup
tic
clear all;
close all;

%%
% Setup variables

bot= -10;
stp= .25;
top=  10;

[x,y]=meshgrid(bot:stp:top);

dummy=(bot:stp:top);

%%
% Define surface
z=x.*sin(x)+y.*sin(y)+1*randn(size(x,1),size(x,1));

%%
% Plot base surface
surf(x,y,z);
hold on;
view([0 90]);
axis([-10 10 -10 10 -20 20]);


%% Index Across Surf
%{
h1=plot3(0,0,0,'.');
set(h1,'Markersize',40);
max=z(1,1);

for y=1:length(dummy)
   for x=1:length(dummy)
       if(z(y,x) > max)
        max=z(y,x);
        set(h1,'ydata',dummy(y));
        set(h1,'xdata',dummy(x));
        set(h1,'zdata',z(y,x));
        title(num2str(z(y,x)));
        pause(.1);
       end       
   end
end
%}

%% Use PSO 
% Generate array of points

t(1:10)=point(dummy);

for i=1:10
    t(i)=t(i).plop(z);
end
mymax.z=0;
mymax.x=0;
mymax.y=0;
mymax.h=plot3(mymax.x,mymax.y,mymax.z,'.r');
set(mymax.h,'markersize',20);

%% 
% PSO loop
for x=1:500
    %%
    % Locate most happy point
    % Plot point happyness
    for i=1:10
        if(t(i).zloc > mymax.z )
            mymax.z=t(i).zloc;
            mymax.x=t(i).xloc;
            mymax.y=t(i).yloc;
            set(mymax.h,'xdata',mymax.x,'ydata',mymax.y,'zdata',mymax.z);
        end
    end
    
    %%
    % Move all points twords that point
    for i=1:10
        t(i)=t(i).move(mymax.x,mymax.y,z);
    end
    pause(.1);
    
    %%
    % Cull points that arn't doing much
    for i=1:10
        if(abs(t(i).xloc - mymax.x) < 1 && ...
                abs(t(i).yloc - mymax.y) < 1 && ...
                sqrt(t(i).xvel^2 + t(i).yvel^2) < 2)
            
            t(i)=t(i).plop(z);
        end
    end
end
abs(mymax.z-max(max(z)))/max(max(z))*100 %#ok<NOPTS>
toc